/**
 * @file euler_kmeans++.h
 * @author circleup (circleup@foxmial.com)
 * @brief 基于欧拉核的 K-means++ 算法
 * @version 0.1
 * @date 2020-07-28
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include "global.h"

#include "screen_driver.h"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>

#include <sensor_msgs/PointCloud2.h>
#include <ros/ros.h>

/**
 * @brief 将实数点映射到复数空间的参数
 * 
 */
#define alpha 1

/**
 * @brief 根号二
 * 
 */
#define sqrt2reciprocal  0.7071067811865

/**
 * @brief 聚类中心个数
 * 
 */
#define K 10

/**
 * @brief 迭代次数
 * 
 */
#define ITERATIONS_MAX 15

/**
 * @brief 复数
 * 
 */
typedef struct
{
  double real;
  double imaginary;
}CPMPLEX;

/**
 * @brief 复数点
 * 
 */
typedef struct
{
  CPMPLEX x;
  CPMPLEX y;
}POINT_COMPLEX;

/**
 * @brief 点云团结构体
 * 
 */
typedef struct
{
  std::vector<uint16_t> indices;
  POINT_COMPLEX euler_mean_center;
}POINTCLOUDINDICES;

class euler_kmeans_plus2
{
public:

  /**
   * @brief 输出点云到rviz
   * 
   */
  sensor_msgs::PointCloud2 point_cloud_out;

  /**
   * @brief 定义 cluster 节点
   * 
   */
  ros::NodeHandle cluster;

  /**
   * @brief 发布点云到 notground_cloud 话题
   * 
   */
  ros::Publisher pub = cluster.advertise<sensor_msgs::PointCloud2>("/notground_cloud", 2);

  /**
   * @brief 输入点云指针
   * 
   */
  pcl::PointCloud<pcl::PointXYZ>::ConstPtr pointcloud_in;

  /**
   * @brief 初始聚类中心
   * 
   */
  std::vector<POINT_COMPLEX> initial_point;

  /**
   * @brief 下一个聚类中心概率
   * 
   */
  std::vector<double> probability;

  /**
   * @brief 点云团结构体
   * 
   */
  std::vector<POINTCLOUDINDICES> cluster_indices;

public:

  /**
   * @brief init object
   * 
   */
  euler_kmeans_plus2(/* args */);

  /**
   * @brief Destroy the euler kmeans plus2 object
   * 
   */
  ~euler_kmeans_plus2();

  /**
   * @brief 计算两点的欧拉距离
   * 
   * @param param1 第一个点 欧几里得坐标系
   * @param param2 第二个点 欧几里得坐标系
   * @return double 距离
   */
  static double DistanceEuler(pcl::PointXYZ param1, pcl::PointXYZ param2);

  /**
   * @brief 计算两点的欧拉距离
   * 
   * @param param1 欧几里得坐标系
   * @param param2 RKHS 坐标系
   * @return double 距离
   */
  static double DistanceEuler(pcl::PointXYZ param1, POINT_COMPLEX param2);

  /**
   * @brief 把某一点通过欧拉核映射到复数空间（未除根号二）
   * 
   * @param param 输入点坐标
   * @return POINT_COMPLEX 输入点的复数坐标
   */
  static POINT_COMPLEX PointXYZTOComplex(pcl::PointXYZ param);

  /**
   * @brief 设置输入点云
   * 
   * @param PointCloudConstPtr 
   */
  void SetPointCloudIn(pcl::PointCloud<pcl::PointXYZ>::ConstPtr PointCloudConstPtr);

  /**
   * @brief 点云数据归一化
   * 
   */
  void PonitCloudNormalization(void);

  /**
   * @brief 初始化聚类中心
   * 
   */
  void InitKClusterCenter(void);

  /**
   * @brief 迭代点云中心
   * 
   */
  void ClusterCenterIteration(void);

};


